﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.Drawing.Drawing2D;
using System.Drawing.Imaging;

namespace FourKill.Engine
{
    public enum FadeDirection
    {
        In,
        Out,
        None
    }

    public static class Utility
    {
        private const double DEG2RAD = Math.PI / 180d;
        private const double RAD2DEG = 180d / Math.PI;

        public static PlacementRule CurrentPlacementRule = PlacementRule.TwoDimensionalFlat;

        public static readonly double _45 = Utility.Radians(45);
        public static readonly double _90 = Utility.Radians(90);
        public static readonly double _135 = Utility.Radians(135);
        public static readonly double _180 = Utility.Radians(180);
        public static readonly double _225 = Utility.Radians(225);
        public static readonly double _270 = Utility.Radians(270);
        public static readonly double _315 = Utility.Radians(315);
        public static readonly double _360 = Utility.Radians(360);

        /// <summary>
        /// Radians the specified degrees.
        /// </summary>
        /// <param name="degrees">The degrees.</param>
        /// <returns></returns>
        public static double Radians(double degrees)
        {
            return degrees * DEG2RAD;
        }

        /// <summary>
        /// Degrees the specified radians.
        /// </summary>
        /// <param name="radians">The radians.</param>
        /// <returns></returns>
        public static double Degrees(double radians)
        {
            return radians * RAD2DEG;
        }

        /// <summary>
        /// Resizes the image.
        /// </summary>
        /// <param name="w">The w.</param>
        /// <param name="h">The h.</param>
        /// <param name="sourceImage">The source image.</param>
        /// <returns></returns>
        public static Bitmap ResizeImage(int w, int h, Bitmap sourceImage)
        {
            if (w == sourceImage.Width && h == sourceImage.Height) return sourceImage;
            else if (w > 0 && h > 0)
            {
                var resizedBmp = new Bitmap(w, h);

                if (w > 0 && h > 0)
                {
                    resizedBmp = new Bitmap(w, h);
                    using (Graphics gr = Graphics.FromImage(resizedBmp))
                    {
                        gr.SmoothingMode = SmoothingMode.AntiAlias;
                        gr.InterpolationMode = InterpolationMode.NearestNeighbor;
                        gr.PixelOffsetMode = PixelOffsetMode.HighQuality;
                        gr.DrawImage(sourceImage, new Rectangle(0, 0, w, h));
                    }
                }
                return resizedBmp;
            }
            else
            {
                throw new Exception("Height or Width of image cannot be zero!");
            }
        }

        public static bool ClipPolygonOnYAxis(List<IPlacement> polygon, double xShift = 0)
        {
            List<IPlacement> fixedPoints = new List<IPlacement>();
            IPlacement point,
            prevPoint = polygon[polygon.Count - 1];
            for (int i = 0; i < polygon.Count; i++)
            {
                point = polygon[i];
                double prevX = prevPoint.X - xShift;
                double currX = point.X - xShift;
                // do check on y...
                if (prevX >= 0)
                {
                    fixedPoints.Add(prevPoint);
                }

                if ((prevX >= 0 && currX < 0) || (prevX < 0 && currX >= 0))
                {
                    // Slope intercept crap: y = mx + b => b = y - mx, m = (y2-y1) / (x2-x1)
                    var yIntercept = point.Y - ((prevPoint.Y - point.Y) / (prevX - currX)) * currX;
                    var zIntercept = point.Z - ((prevPoint.Z - point.Z) / (prevX - currX)) * currX;
                    Placement pnew = new Placement();
                    prevPoint.CopyInto(pnew);
                    pnew.X = xShift;
                    pnew.Y = yIntercept;
                    pnew.Z = zIntercept;
                    fixedPoints.Add(pnew);
                }

                prevPoint = point;
            }
            polygon.Clear();
            polygon.AddRange(fixedPoints);

            return (fixedPoints.Count >= 3);
        }

        public static void FindRelativeLocation3D(IPlacement origin, ICoordinates3D coordinates, ref IPlacement relativePlacement)
        {
            Coordinates3D.CopyInto(coordinates, relativePlacement);
            double relativeAngleToX, relativeAngle, //relativeBearing,
                translatedX = (coordinates.X - origin.X),
                translatedY = (coordinates.Y - origin.Y),
                reverseBearing = -1 * origin.Bearing;

            relativePlacement.X = (Math.Cos(reverseBearing) * translatedX) - (Math.Sin(reverseBearing) * translatedY);
            relativePlacement.Y = (Math.Sin(reverseBearing) * translatedX) + (Math.Cos(reverseBearing) * translatedY);

            relativeAngleToX = Math.Atan2(Math.Abs(relativePlacement.Y), Math.Abs(relativePlacement.X)) *
                (relativePlacement.Y < 0 && relativePlacement.X > 0 ? -1d : 1d);

            if (relativePlacement.X < 0 && relativePlacement.Y < 0) relativeAngle = Math.Abs(relativeAngleToX) + Math.PI;
            else if (relativePlacement.X > 0 && relativePlacement.Y < 0) relativeAngle = 2 * Math.PI - Math.Abs(relativeAngleToX);
            else if (relativePlacement.X < 0 && relativePlacement.Y > 0) relativeAngle = Math.PI - Math.Abs(relativeAngleToX);
            else relativeAngle = relativeAngleToX;

            relativePlacement.AngleFromCamera = relativeAngle;
            if (relativePlacement.AngleFromCamera < 0) relativePlacement.AngleFromCamera = Utility._360 + relativePlacement.AngleFromCamera;
            relativePlacement.AngleFromCamera = relativePlacement.AngleFromCamera > Utility._360 ? (Utility._360 % relativePlacement.AngleFromCamera) : relativePlacement.AngleFromCamera;

        }

        /// <summary>
        /// Finds the relative location.
        /// </summary>
        /// <param name="origin">The origin.</param>
        /// <param name="objectLocation">The object location.</param>
        /// <returns></returns>
        public static void FindRelativeLocation(IPlacement origin, IPlacement objectLocation, ref IPlacement relativePlacement)
        {
            Placement.CopyInto(objectLocation, relativePlacement);
            double relativeAngleToX, relativeAngle, //relativeBearing,
                translatedX = (objectLocation.X - origin.X),
                translatedY = (objectLocation.Y - origin.Y),
                reverseBearing = -1 * origin.Bearing;

            relativePlacement.X = (Math.Cos(reverseBearing) * translatedX) - (Math.Sin(reverseBearing) * translatedY);
            relativePlacement.Y = (Math.Sin(reverseBearing) * translatedX) + (Math.Cos(reverseBearing) * translatedY);

            relativeAngleToX = Math.Atan2(Math.Abs(relativePlacement.Y), Math.Abs(relativePlacement.X)) *
                (relativePlacement.Y < 0 && relativePlacement.X > 0 ? -1d : 1d);

            if (relativePlacement.X < 0 && relativePlacement.Y < 0) relativeAngle = Math.Abs(relativeAngleToX) + Math.PI;
            else if (relativePlacement.X > 0 && relativePlacement.Y < 0) relativeAngle = 2 * Math.PI - Math.Abs(relativeAngleToX);
            else if (relativePlacement.X < 0 && relativePlacement.Y > 0) relativeAngle = Math.PI - Math.Abs(relativeAngleToX);
            else relativeAngle = relativeAngleToX;

            relativePlacement.AngleFromCamera = relativeAngle;
            if (relativePlacement.AngleFromCamera < 0) relativePlacement.AngleFromCamera = Utility._360 + relativePlacement.AngleFromCamera;
            relativePlacement.AngleFromCamera = relativePlacement.AngleFromCamera > Utility._360 ? (Utility._360 % relativePlacement.AngleFromCamera) : relativePlacement.AngleFromCamera;


            relativePlacement.Bearing = objectLocation.Bearing - origin.Bearing;
            if (relativePlacement.Bearing < 0) relativePlacement.Bearing = Utility._360 + relativePlacement.Bearing;
            relativePlacement.Bearing = relativePlacement.Bearing > Utility._360 ? (Utility._360 % relativePlacement.Bearing) : relativePlacement.Bearing;

            //RelativeDistance = Math.Sqrt(Math.Pow(RelativeX, 2) + Math.Pow(RelativeY, 2));

            //return relativePlacement;
        }


        public static double AngleToPoint(this Coordinates3D first, Coordinates3D toPoint)
        {
            return Math.Atan2(toPoint.Y - first.Y, toPoint.X - first.X); //* 180 / Math.PI;
        }
        /// <summary>
        /// Finds the distance using the 2D distance formula.
        /// </summary>
        /// <param name="a">Point A.</param>
        /// <param name="b">Point B.</param>
        /// <returns></returns>
        public static double FindDistance2D(ICoordinates2D a, ICoordinates2D b)
        {
            var distance = Math.Sqrt(Math.Pow((a.X - b.X), 2) + Math.Pow((a.Y - b.Y), 2));

            return distance;
        }

        /// <summary>
        /// Finds the distance using the 3D distance formula.
        /// </summary>
        /// <param name="a">Point A.</param>
        /// <param name="b">Point B.</param>
        /// <returns></returns>
        public static double FindDistance3D(ICoordinates3D a, ICoordinates3D b)
        {
            double longLeg = FindDistance2D(a, b),
                shortLeg = Math.Abs(a.Z - b.Z),
                hypotenuse = shortLeg == 0 ? longLeg : Math.Sqrt(Math.Pow(longLeg, 2) + Math.Pow(shortLeg, 2));

            return hypotenuse;
        }

        /// <summary>
        /// Expands hard points into points based on the movement distance.
        /// </summary>
        public static List<Coordinates3D> ExpandPath(this List<Coordinates3D> path, double interval)
        {
            // Temporary
            int Z = 0;
            List<Coordinates3D> expandedPath = new List<Coordinates3D>();
            for (int i = 0; i < path.Count - 1; i++)
            {
                double angle = path[i].AngleToPoint(path[i + 1]);
                expandedPath.Add(path[i]);

                double distanceBetweenHardPoints = Utility.FindDistance3D(path[i], path[i + 1]);
                for (int j = 0; j < (distanceBetweenHardPoints - interval) / interval; j++)
                {
                    double x = path[i].X + interval * (j + 1) * Math.Cos(angle);
                    double y = path[i].Y + interval * (j + 1) * Math.Sin(angle);

                    // Add new dynamic point.
                    expandedPath.Add(new Coordinates3D(x, y, Z));
                }
            }
            return expandedPath;
        }

        /// <summary>
        /// 
        /// </summary>
        public enum PlacementRule
        {
            TwoDimensionalFlat = 0,
            TwoDimensional,
            ThreeDimensional
        }

        public static double? GetAngle(ICoordinates2D c1, ICoordinates2D c2)
        {
            double? angle = null;
            if (c1.X != c2.X || c1.Y != c2.Y)
            {
                angle = Math.Atan((c1.X - c2.X) / (c1.Y - c2.Y));
            }
            return angle;
        }

        /// <summary>
        /// Calculate2s the D view location.
        /// </summary>
        /// <param name="camera">The camera.</param>
        /// <param name="objectLocation">The object location.</param>
        /// <returns></returns>
        public static void CalculateObjectViewLocation(
            ref IObjectViewLocation viewLocation,
            ICamera camera,
            IViewPort viewPort,
            IPlacement objectLocation,
            bool calculateIfOffScreen = false)
        {
            viewLocation.IsVisible = true;
            try
            {
                IPlacement relativeLocation = new Placement();
                FindRelativeLocation(camera, objectLocation, ref relativeLocation);
                viewLocation.Placement = relativeLocation;

                //double angle;
                //if (relativeLocation.Bearing > _360) System.Diagnostics.Debugger.Break();
                double angle = (relativeLocation.AngleFromCamera < _180) ? relativeLocation.AngleFromCamera : _360 - relativeLocation.AngleFromCamera;

                if (relativeLocation.X <= 0 ||
                    (angle > (viewPort.HorizontalViewableAngle / 2) + Radians(2)))
                {
                    viewLocation.IsVisible = true;
                }

                bool doCalculations = (viewLocation.IsVisible || (calculateIfOffScreen && relativeLocation.X > 0));
                if (doCalculations)
                {

                    // Pick one of the three above.
                    double unitDistance,
                        top = relativeLocation.Z + relativeLocation.Height,
                        bottom = relativeLocation.Z,
                        cameraHeight = camera.Height + camera.Z;

                    Coordinates3D c3dOrigin = Coordinates3D.Clone(Coordinates3D.Origin);
                    c3dOrigin.Z = cameraHeight;

                    double unitDistance3D = FindDistance3D(c3dOrigin, relativeLocation) * (relativeLocation.X > 0 ? 1 : -1);
                    switch (CurrentPlacementRule)
                    {
                        case PlacementRule.TwoDimensional:
                            unitDistance = FindDistance2D(c3dOrigin, relativeLocation) * (relativeLocation.X > 0 ? 1 : -1);
                            break;
                        case PlacementRule.ThreeDimensional:
                            unitDistance = unitDistance3D;
                            break;
                        default:
                            unitDistance = relativeLocation.X;
                            break;
                    }

                    if (unitDistance < viewPort.DistanceToView)
                        viewLocation.IsVisible = false;
                    else
                    {
                        viewLocation.Distance = unitDistance;
                        viewLocation.Distance3D = unitDistance3D;
                        double pointOffsetFromHorizon_Ground = (cameraHeight / unitDistance) * viewPort.DistanceToView;
                        double pointOffsetFromHorizon_Bottom = (Math.Abs(bottom - cameraHeight) / unitDistance) * viewPort.DistanceToView;
                        double pointOffsetFromHorizon_Top = (Math.Abs(top - cameraHeight) / unitDistance) * viewPort.DistanceToView;

                        double pointOffsetFromVerticalHorizon = (relativeLocation.Y / unitDistance) * viewPort.DistanceToView;

                        // Shadow
                        double unitShadowHeight = Math.Abs(((cameraHeight / (unitDistance + relativeLocation.Width)) * viewPort.DistanceToView) - pointOffsetFromHorizon_Ground);

                        pointOffsetFromHorizon_Ground = (cameraHeight - pointOffsetFromHorizon_Ground);

                        pointOffsetFromHorizon_Bottom = (bottom > cameraHeight)
                            ? (cameraHeight + pointOffsetFromHorizon_Bottom) : (cameraHeight - pointOffsetFromHorizon_Bottom);

                        pointOffsetFromHorizon_Top = (top > cameraHeight)
                            ? (cameraHeight + pointOffsetFromHorizon_Top) : (cameraHeight - pointOffsetFromHorizon_Top);

                        //int horizontalOffset = Convert.ToInt32(pointOffsetFromVerticalHorizon * viewPort.Scale) * (relativeLocation.Bearing >= 0 ? -1 : 1);
                        int horizontalOffset = Convert.ToInt32(pointOffsetFromVerticalHorizon * viewPort.Scale) * -1;

                        // shadow calc
                        int shadowHeight = Convert.ToInt32(unitShadowHeight * viewPort.Scale);

                        var objectRectangle = new Rectangle();
                        objectRectangle.Height = Convert.ToInt32((pointOffsetFromHorizon_Top - pointOffsetFromHorizon_Bottom) * viewPort.Scale);
                        objectRectangle.Width = Convert.ToInt32((objectLocation.Width * objectRectangle.Height) / objectLocation.Height);

                        objectRectangle.Y = (viewPort.PixelSize.Height / 2) + Convert.ToInt32((cameraHeight - pointOffsetFromHorizon_Top) * viewPort.Scale);
                        objectRectangle.X = (viewPort.PixelSize.Width / 2) - (objectRectangle.Width / 2) + horizontalOffset;

                        var shadowRectangle = new Rectangle();
                        shadowRectangle.Height = shadowHeight;
                        shadowRectangle.Width = objectRectangle.Width;
                        shadowRectangle.X = objectRectangle.X;
                        shadowRectangle.Y = (viewPort.PixelSize.Height / 2) + Convert.ToInt32((cameraHeight - pointOffsetFromHorizon_Ground) * viewPort.Scale) - (shadowHeight / 2);

                        viewLocation.ObjectRectangle = objectRectangle;
                        viewLocation.ShadowRectangle = shadowRectangle;
                    }
                }
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine("***********************************");
                System.Diagnostics.Debug.WriteLine(ex.Message + "\r\n" + ex.StackTrace);
                System.Diagnostics.Debugger.Break();
            }
        }


        /// <summary>
        /// Calculate2s the D view location.
        /// </summary>
        /// <param name="camera">The camera.</param>
        /// <param name="objectLocation">The object location.</param>
        /// <returns></returns>
        public static void CalculatePointViewLocation(
            ref IPointViewLocation viewLocation,
            ICamera camera,
            IViewPort viewPort,
            ICoordinates3D objectLocation,
            bool calculateIfOffScreen = false)
        {
            viewLocation.IsVisible = true;
            try
            {
                IPlacement relativeLocation;
                if (viewLocation.Placement == null)
                {
                    relativeLocation = new Placement();
                    FindRelativeLocation3D(camera, objectLocation, ref relativeLocation);
                    viewLocation.Placement = relativeLocation;
                }
                else 
                {
                    relativeLocation = viewLocation.Placement;
                }

                //double angle;
                //if (relativeLocation.Bearing > _360) System.Diagnostics.Debugger.Break();
                double angle = (relativeLocation.AngleFromCamera < _180) ? relativeLocation.AngleFromCamera : _360 - relativeLocation.AngleFromCamera;
                
                if (relativeLocation.X <= 0 ||
                    (angle > (viewPort.HorizontalViewableAngle / 2) + Radians(2)))
                {
                    viewLocation.IsVisible = true;
                }

                bool doCalculations = (viewLocation.IsVisible || (calculateIfOffScreen && relativeLocation.X > 0));
                if (doCalculations)
                {
                    // Pick one of the three above.
                    double unitDistance,
                        top = relativeLocation.Z + relativeLocation.Height,
                        bottom = relativeLocation.Z,
                        cameraHeight = camera.Height + camera.Z;

                    Coordinates3D c3dOrigin = Coordinates3D.Clone(Coordinates3D.Origin);
                    c3dOrigin.Z = cameraHeight;

                    double unitDistance3D = FindDistance3D(c3dOrigin, relativeLocation) * (relativeLocation.X > 0 ? 1 : -1);
                    switch (CurrentPlacementRule)
                    {
                        case PlacementRule.TwoDimensional:
                            unitDistance = FindDistance2D(c3dOrigin, relativeLocation) * (relativeLocation.X > 0 ? 1 : -1);
                            break;
                        case PlacementRule.ThreeDimensional:
                            unitDistance = unitDistance3D;
                            break;
                        default:
                            unitDistance = relativeLocation.X;
                            break;
                    }

                    if (unitDistance < viewPort.DistanceToView)
                        viewLocation.IsVisible = false;
                    else
                    {
                        viewLocation.Distance = unitDistance;
                        viewLocation.Distance3D = unitDistance3D;
                        double pointOffsetFromHorizon_Ground = (cameraHeight / unitDistance) * viewPort.DistanceToView;
                        double pointOffsetFromHorizon_Bottom = (Math.Abs(bottom - cameraHeight) / unitDistance) * viewPort.DistanceToView;
                        double pointOffsetFromHorizon_Top = (Math.Abs(top - cameraHeight) / unitDistance) * viewPort.DistanceToView;

                        // This could be done a number of different ways.. need to look into it further...
                        // Need to try different values of unitDistance (2D, 3D, XAxis)
                        double pointOffsetFromVerticalHorizon = (relativeLocation.Y / unitDistance) * viewPort.DistanceToView;

                        // Shadow
                        double unitShadowHeight = Math.Abs(((cameraHeight / (unitDistance + relativeLocation.Width)) * viewPort.DistanceToView) - pointOffsetFromHorizon_Ground);

                        pointOffsetFromHorizon_Ground = (cameraHeight - pointOffsetFromHorizon_Ground);

                        pointOffsetFromHorizon_Bottom = (bottom > cameraHeight)
                            ? (cameraHeight + pointOffsetFromHorizon_Bottom) : (cameraHeight - pointOffsetFromHorizon_Bottom);

                        pointOffsetFromHorizon_Top = (top > cameraHeight)
                            ? (cameraHeight + pointOffsetFromHorizon_Top) : (cameraHeight - pointOffsetFromHorizon_Top);

                        //int horizontalOffset = Convert.ToInt32(pointOffsetFromVerticalHorizon * viewPort.Scale) * (relativeLocation.Bearing >= 0 ? -1 : 1);
                        int horizontalOffset = Convert.ToInt32(pointOffsetFromVerticalHorizon * viewPort.Scale) * -1;

                        // shadow calc
                        int shadowHeight = Convert.ToInt32(unitShadowHeight * viewPort.Scale);

                        var point = new Point();

                        point.Y = (viewPort.PixelSize.Height / 2) + Convert.ToInt32((cameraHeight - pointOffsetFromHorizon_Top) * viewPort.Scale);
                        point.X = (viewPort.PixelSize.Width / 2) + horizontalOffset;

                        viewLocation.Point = point;

                        var shadowPoint = new Point();

                        shadowPoint.Y = (viewPort.PixelSize.Height / 2) + Convert.ToInt32((cameraHeight - pointOffsetFromHorizon_Ground) * viewPort.Scale);
                        shadowPoint.X = point.X;


                        viewLocation.ShadowPoint = shadowPoint;
                    }
                }
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine("***********************************");
                System.Diagnostics.Debug.WriteLine(ex.Message + "\r\n" + ex.StackTrace);
                System.Diagnostics.Debugger.Break();
            }
        }

        /// <summary>
        /// Creates the rounded rectangle path.
        /// </summary>
        /// <param name="rect">The rect.</param>
        /// <param name="cornerRadius">The corner radius.</param>
        /// <returns></returns>
        public static GraphicsPath CreateRoundedRectanglePath(Rectangle rect, int cornerRadius)
        {
            GraphicsPath roundedRect = new GraphicsPath();
            roundedRect.AddArc(rect.X, rect.Y, cornerRadius * 2, cornerRadius * 2, 180, 90);
            roundedRect.AddLine(rect.X + cornerRadius, rect.Y, rect.Right - cornerRadius * 2, rect.Y);
            roundedRect.AddArc(rect.X + rect.Width - cornerRadius * 2, rect.Y, cornerRadius * 2, cornerRadius * 2, 270, 90);
            roundedRect.AddLine(rect.Right, rect.Y + cornerRadius * 2, rect.Right, rect.Y + rect.Height - cornerRadius * 2);

            roundedRect.AddArc(rect.X + rect.Width - cornerRadius * 2, rect.Y + rect.Height - cornerRadius * 2, cornerRadius * 2, cornerRadius * 2, 0, 90);

            roundedRect.AddLine(rect.Right - cornerRadius * 2, rect.Bottom, rect.X + cornerRadius * 2, rect.Bottom);
            roundedRect.AddArc(rect.X, rect.Bottom - cornerRadius * 2, cornerRadius * 2, cornerRadius * 2, 90, 90);
            roundedRect.AddLine(rect.X, rect.Bottom - cornerRadius * 2, rect.X, rect.Y + cornerRadius * 2);
            roundedRect.CloseFigure();
            return roundedRect;
        }

        public static Bitmap CreateMask(Bitmap image, Color transparencyColor)
        {
            Bitmap mask = new Bitmap(image.Width, image.Height);
            for (int y = 0; y < image.Height; y++)
            {
                for (int x = 0; x < image.Width; x++)
                {
                    Color pixel = image.GetPixel(x, y);
                    if (pixel.A == transparencyColor.A &&
                        pixel.R == transparencyColor.R &&
                        pixel.G == transparencyColor.G &&
                        pixel.B == transparencyColor.B)
                    {
                        mask.SetPixel(x, y, Color.White);
                    }
                    else
                    {
                        mask.SetPixel(x, y, Color.Black);
                    }
                }
            }
            mask.Save(@"masktest.png", ImageFormat.Png);
            image.Save(@"realimage.png", ImageFormat.Png);
            return mask;
        }

        /// <summary>
        /// Adjusts the brightness matrix.
        /// </summary>
        /// <param name="img">The img.</param>
        /// <param name="value">The value.</param>
        public static void AdjustBrightnessMatrix(Bitmap img, int value)
        {
            if (value == 0) // No change, so just return
                return;

            float sb = (float)value / 255F;
            float[][] colorMatrixElements =
                  {
                        new float[] {1,  0,  0,  0, 0},
                        new float[] {0,  1,  0,  0, 0},
                        new float[] {0,  0,  1,  0, 0},
                        new float[] {0,  0,  0,  1, 0},
                        new float[] {sb, sb, sb, 1, 1}
                                          };

            ColorMatrix cm = new ColorMatrix(colorMatrixElements);
            ImageAttributes imgattr = new ImageAttributes();
            Rectangle rc = new Rectangle(0, 0, img.Width, img.Height);
            Graphics g = Graphics.FromImage(img);
            g.InterpolationMode = InterpolationMode.HighQualityBicubic;
            imgattr.SetColorMatrix(cm);
            g.DrawImage(img, rc, 0, 0, img.Width, img.Height, GraphicsUnit.Pixel, imgattr);

            //Clean everything up
            imgattr.Dispose();
            g.Dispose();

        }
    }
}
